#include "filter_new.hpp"
#include <numeric>

// 环形缓冲区实现
CircularBufferFilter::CircularBufferFilter() {
  // 初始化数组
  for (size_t i = 0; i < FILTER_SIZE; ++i) {
    points_[i] = cv::Point2f(0,0);
    weights_[i] = 0.0f;
  }
}

void CircularBufferFilter::addData(cv::Point2f point, float weight) {
  points_[index_] = point;
  weights_[index_] = weight;

  // 更新索引（环形）
  index_ = (index_ + 1) % FILTER_SIZE;

  // 更新大小
  if (size_ < FILTER_SIZE) {
    size_++;
  }
}

cv::Point2f CircularBufferFilter::getFilteredPoint() const {
  if (size_ == 0) {
    return cv::Point2f();
  }

  // 加权平均滤波
  float total_weight = 0.0f;
  float weighted_x = 0.0f;
  float weighted_y = 0.0f;

  for (size_t i = 0; i < size_; ++i) {
    weighted_x += points_[i].x * weights_[i];
    weighted_y += points_[i].y * weights_[i];
    total_weight += weights_[i];
  }

  if (total_weight > 0.0f) {
    return cv::Point2f(weighted_x / total_weight, weighted_y / total_weight);
  } else {
    // 如果权重为0，使用简单平均
    float avg_x = 0.0f, avg_y = 0.0f;
    for (size_t i = 0; i < size_; ++i) {
      avg_x += points_[i].x;
      avg_y += points_[i].y;
    }
    return cv::Point2f(avg_x / static_cast<float>(size_),
                       avg_y / static_cast<float>(size_));
  }
}

// // 双端队列实现
// DequeFilter::DequeFilter(size_t max_size) : max_size_(max_size) {}

// void DequeFilter::addData(cv::Point2f point, float weight) {
//   points_.push_back(point);
//   weights_.push_back(weight);

//   // 如果超过最大大小，移除最旧的数据
//   if (points_.size() > max_size_) {
//     points_.pop_front();
//     weights_.pop_front();
//   }
// }

// cv::Point2f DequeFilter::getFilteredPoint() const {
//   if (points_.empty()) {
//     return cv::Point2f();
//   }

//   // 加权平均滤波
//   float total_weight = 0.0f;
//   float weighted_x = 0.0f;
//   float weighted_y = 0.0f;

//   for (size_t i = 0; i < points_.size(); ++i) {
//     weighted_x += points_[i].x * weights_[i];
//     weighted_y += points_[i].y * weights_[i];
//     total_weight += weights_[i];
//   }

//   if (total_weight > 0.0f) {
//     return cv::Point2f(weighted_x / total_weight, weighted_y / total_weight);
//   } else {
//     // 如果权重为0，使用简单平均
//     float avg_x = 0.0f, avg_y = 0.0f;
//     for (size_t i = 0; i < points_.size(); ++i) {
//       avg_x += points_[i].x;
//       avg_y += points_[i].y;
//     }
//     return cv::Point2f(avg_x / static_cast<float>(points_.size()),
//                        avg_y / static_cast<float>(points_.size()));
//   }
// }

// // 向量实现
// VectorFilter::VectorFilter(size_t max_size) : max_size_(max_size) {
//   points_.reserve(max_size);
//   weights_.reserve(max_size);
// }

// void VectorFilter::addData(cv::Point2f point, float weight) {
//   if (points_.size() >= max_size_) {
//     // 移除最旧的数据（头部）
//     points_.erase(points_.begin());
//     weights_.erase(weights_.begin());
//   }
//   points_.push_back(point);
//   weights_.push_back(weight);
// }

// cv::Point2f VectorFilter::getFilteredPoint() const {
//   if (points_.empty()) {
//     return cv::Point2f();
//   }

//   // 加权平均滤波
//   float total_weight = 0.0f;
//   float weighted_x = 0.0f;
//   float weighted_y = 0.0f;

//   for (size_t i = 0; i < points_.size(); ++i) {
//     weighted_x += points_[i].x * weights_[i];
//     weighted_y += points_[i].y * weights_[i];
//     total_weight += weights_[i];
//   }

//   if (total_weight > 0.0f) {
//     return cv::Point2f(weighted_x / total_weight, weighted_y / total_weight);
//   } else {
//     // 如果权重为0，使用简单平均
//     float avg_x = 0.0f, avg_y = 0.0f;
//     for (size_t i = 0; i < points_.size(); ++i) {
//       avg_x += points_[i].x;
//       avg_y += points_[i].y;
//     }
//     return cv::Point2f(avg_x / static_cast<float>(points_.size()),
//                        avg_y / static_cast<float>(points_.size()));
//   }
// }
